#include "../utils.h"
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include "robots_msgs/Nav.h"
#include "robots_msgs/NavStatus.h"

namespace nav{

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

class NavService {
  private:
    ros::NodeHandle nh;
    tf::TransformListener listener;
    ros::Subscriber nav_cmd_sub;
    ros::Subscriber nav_status_sub;

    nav::NavMode last_mode;
    robots_msgs::Nav last_nav_order;
    robots_msgs::NavStatus nav_status;

    std::unique_ptr<MoveBaseClient> movebase_cient;

    void navRequestCB(const robots_msgs::NavConstPtr& nav_order_ptr);
    void navStatusCB(const robots_msgs::NavStatusConstPtr& nav_status_ptr){
        nav_status = *nav_status_ptr;
    }

    ros::Time last_patrolling_timestamp;
    int last_patrolling_index;

    ros::Time last_single_point_timestamp;
    int last_single_point_index;

  public:
    NavService();
    ~NavService(){}
};

}
